#include "Observation.h"
#include "config.h"
#include<fstream>
#include<string>
#include<cmath>

Observation::Observation()
{
	cloud_data = pcl::PointCloud<PointIoType>::Ptr(new pcl::PointCloud<PointIoType>);
}


Observation::~Observation()
{
}

int Observation::executebin2pcd(char *source_dir,const string PCD_OUTPUT_PATH) {
	listFiles(source_dir);
	for (int i = 0; i < obs_list.size(); i++) {
		bin2pcd(source_dir, obs_list[i], PCD_OUTPUT_PATH);
	}
	return 1;
}
int Observation::observationDataInit(const string BIN_INPUT_PATH)
{
	if (listFiles(BIN_INPUT_PATH.data()).size()>0) {
		return 1;
	}
	return 0;
	
}
pcl::PointCloud<PointIoType>::Ptr Observation::getFromBin(const size_t index, const string BIN_INPUT_PATH,bool cut)
{
	if (index > obs_list.size()-1) {
		cout <<"Exception step :" <<index << endl;
		throw "out of range when getFromBin";
	}
	string inputfilename = obs_list[index];
	inputfilename = inputfilename.insert(0, BIN_INPUT_PATH);
	const char* inptfile = inputfilename.data();
	int32_t num = 1000000;
	float *data = (float*)malloc(num * sizeof(float));
	// 
	float *px = data + 0;
	float *py = data + 1;
	float *pz = data + 2;
	float *pr = data + 3;
	// 
	FILE *stream;
	fopen_s(&stream, inptfile, "rb");
	num = fread(data, sizeof(float), num, stream) / 4;//
	fclose(stream);
	if (num != 0)
		cloud_data->clear();
	else
		return nullptr;

	PointIoType point;
	for (int32_t i = 0; i < num; i++)
	{
		if (cut) {
			if (sqrt((*px)*(*px) + (*py)*(*py) + (*pz)*(*pz)) < threshold) {
				point.x = *px;
				point.y = *py;
				point.z = *pz;
				//we do not use the intensity .
				//if should use change the pointIoType and add the code: point.intensity = *pr;
				cloud_data->points.push_back(point);
			}
		}else {
		
			point.x = *px;
			point.y = *py;
			point.z = *pz;
			//we do not use the intensity .
			//if should use change the pointIoType and add the code: point.intensity = *pr;
			cloud_data->points.push_back(point);
		}
		px += 4; py += 4; pz += 4; pr += 4;
	}
	free(data);
	return cloud_data;
}





vector<string> Observation::listFiles(const char * dir)
{
	char dirNew[200];
	strcpy(dirNew, dir);
	strcat(dirNew, "\\*.*");    // 

	intptr_t handle;
	_finddata_t findData;

	handle = _findfirst(dirNew, &findData);
	if (handle == -1)        // 
		return obs_list;

	do
	{
		if (findData.attrib & _A_SUBDIR)
		{
			if (strcmp(findData.name, ".") == 0 || strcmp(findData.name, "..") == 0)
				continue;

			cout << findData.name << "\t<dir>\n";

			// 
			strcpy(dirNew, dir);
			strcat(dirNew, "\\");
			strcat(dirNew, findData.name);

			listFiles(dirNew);
		}
		else {
			//cout << findData.name << "\t" << findData.size << " bytes.\n";
			obs_list.push_back(findData.name);
		}
	} while (_findnext(handle, &findData) == 0);

	_findclose(handle);    // 
	return obs_list;
}

void Observation::bin2pcd(const char * dir, const string filename,const string PCD_OUTPUT_PATH) {
	string inputfilename = filename;
	inputfilename = inputfilename.insert(0, dir);
	const char* inptfile = inputfilename.data();
	int32_t num = 1000000;
	float *data = (float*)malloc(num * sizeof(float));
	// 
	float *px = data + 0;
	float *py = data + 1;
	float *pz = data + 2;
	float *pr = data + 3;//
	// 
	FILE *stream;
	fopen_s(&stream, inptfile, "rb");
	num = fread(data, sizeof(float), num, stream) / 4;//
	fclose(stream);
	int32_t after_erase_num = 0;
	int distance_threshold=20;
	float *px_t = px;
	float *py_t = py;
	float *pz_t = pz;
	float *pr_t = pr;//
	for (int32_t i = 0; i < num; i++)
	{	
		//setting a threshold according to the distance between the point and centre to decrease the points 
		double distance = sqrt((*px_t)*(*px_t) + (*py_t)*(*py_t) + (*pz_t)*(*pz_t));
		if (distance < distance_threshold) {
			after_erase_num += 1;
		}
		px_t += 4; py_t += 4; pz_t += 4; pr_t += 4;

	}
	const char* outfile;
	string outputfilename = filename;
	outputfilename = outputfilename.replace(outputfilename.find(".") + 1, 3, "pcd");
	outfile = outputfilename.insert(0, PCD_OUTPUT_PATH).data();
	FILE *writePCDStream;
	fopen_s(&writePCDStream, outfile, "wb");
	fprintf(writePCDStream, "VERSION 0.7\n");//
	fprintf(writePCDStream, "FIELDS x y z\n");//
	fprintf(writePCDStream, "SIZE 4 4 4\n");//
	fprintf(writePCDStream, "TYPE F F F\n");//
	fprintf(writePCDStream, "WIDTH %d\n", after_erase_num);//
	fprintf(writePCDStream, "HEIGHT 1\n");//
	fprintf(writePCDStream, "POINTS %d\n", after_erase_num);//
	fprintf(writePCDStream, "DATA ascii\n");//
	
	for (int32_t i = 0; i < num; i++)
	{
		double distance = sqrt((*px)*(*px) + (*py)*(*py) + (*pz)*(*pz));
		if (distance < distance_threshold) {
			fprintf(writePCDStream, "%f %f %f %f\n", *px, *py, *pz, *pr);
		}
		px += 4; py += 4; pz += 4; pr += 4;	
	}
	fclose(writePCDStream);
}

void Observation::transfromMatrix_Reader(vector<Eigen::Matrix4d> &transform, const string POSE_DATA_PATH) {
	string file_path = POSE_DATA_PATH;
	ifstream FileIn(file_path);
	Eigen::Matrix4d pose_data2;
	Eigen::Matrix4d velo2cam, cam2velo;
	cam2velo << 0, 0, 1, 0,
				-1, 0, 0, 0,
				0, -1, 0, 0.08,
				0, 0, 0, 1;
	velo2cam << 0, -1, 0, 0,
				0, 0, -1, 0,
				1, 0, 0, -0.08,
				0, 0, 0, 1;
	
	while (FileIn >> pose_data2(0,0) >> pose_data2(0, 1) >> pose_data2(0, 2) >> pose_data2(0, 3)
		>> pose_data2(1, 0) >> pose_data2(1, 1) >> pose_data2(1, 2) >> pose_data2(1, 3)
		>> pose_data2(2, 0) >> pose_data2(2, 1) >> pose_data2(2, 2) >> pose_data2(2, 3))
	{
		pose_data2(3, 0) = 0;
		pose_data2(3, 1) = 0;
		pose_data2(3, 2) = 0;
		pose_data2(3, 3) = 1;
		pose_data2 = cam2velo * pose_data2 * velo2cam;
		transform.push_back(pose_data2);
	}
	FileIn.close();
}

void Observation::groundTruthReader(vector<Eigen::Matrix4d>& GT, const string POSE_DATA_PATH)
{
	string file_path = POSE_DATA_PATH;
	ifstream FileIn(file_path);
	Eigen::Matrix4d gt_data;
	Eigen::Matrix4d velo2cam, cam2velo;
	cam2velo << 0, 0, 1, 0,
		-1, 0, 0, 0,
		0, -1, 0, 0.08,
		0, 0, 0, 1;
	velo2cam << 0, -1, 0, 0,
		0, 0, -1, 0,
		1, 0, 0, -0.08,
		0, 0, 0, 1;

	while (FileIn >> gt_data(0, 0) >> gt_data(0, 1) >> gt_data(0, 2) >> gt_data(0, 3)
		>> gt_data(1, 0) >> gt_data(1, 1) >> gt_data(1, 2) >> gt_data(1, 3)
		>> gt_data(2, 0) >> gt_data(2, 1) >> gt_data(2, 2) >> gt_data(2, 3))
	{
		gt_data(3, 0) = 0;
		gt_data(3, 1) = 0;
		gt_data(3, 2) = 0;
		gt_data(3, 3) = 1;
		gt_data = cam2velo * gt_data * velo2cam;

		//to trange the matrix to the yaw pitch roll

		GT.push_back(gt_data);
	}
	FileIn.close();
}
